#coding=utf-8
import json
import socket
import threading
import numpy as np
from interfaces.udp.latency import *
import time
'''
    信息中心
'''


class UDP_Data_Bridge:
    '''
        基于UDP的数据接口
    '''
    def __init__(self,parent,lFoot,rFoot,base,lChain,rChain,port_lfoot=8902,port_rfoot=8903,port_base=8901,port_jointcontrol=8886,port_voiceout=8890,port_stampync=8801,ip_console="192.168.0.100",\
                 ip_lfoot='192.168.0.3',ip_rfoot='192.168.0.4',ip_base='192.168.0.11'):
        '''

        :param parent:
        :param lFoot:
        :param rFoot:
        :param base:
        :param port_lfoot:
        :param port_rfoot:
        :param port_base:
        :param port_jointcontrol:
        :param port_voiceout:
        :param ip_console:
        :param ip_lfoot:
        :param ip_rfoot:
        :param ip_base:
        '''
        self.parent=parent  #父对象
        #   映射的对象
        self.lFoot=lFoot
        self.rFoot=rFoot
        self.base=base
        self.lChain=lChain
        self.rChain=rChain
        #   控制端口
        self.port_lfoot=port_lfoot
        self.port_rfoot=port_rfoot
        self.port_base=port_base
        self.port_jointcontrol=port_jointcontrol
        self.port_voiceout=port_voiceout
        self.port_stampsync=port_stampync
        #   IP地址
        self.ip_lfoot=ip_lfoot
        self.ip_rfoot=ip_rfoot
        self.ip_base=ip_base
        #   本地IP
        self.ip=ip_console
        self.alive=True

        '''
            建立各个模块的UDP交互端口
        '''
        self.b_client=socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  #   base_link
        self.l_client=socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  #   左脚
        self.r_client=socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  #   右脚

        self.v_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)    #语音输出
        self.j_client=socket.socket(socket.AF_INET, socket.SOCK_DGRAM)      #关节交互

        self.b_client.bind((self.ip, self.port_base))
        self.l_client.bind((self.ip, self.port_lfoot))
        self.r_client.bind((self.ip, self.port_rfoot))
        self.v_client.bind((self.ip, self.port_voiceout))
        self.j_client.bind((self.ip, self.port_jointcontrol))

        #   开启各个端口的独立线程
        threadPool = []
        '''
            此处添加全部线程，然后自动启动全部线程(接收线程)
        '''
        threadPool.append(threading.Thread(target=self.L_Handler))
        threadPool.append(threading.Thread(target=self.R_Handler))
        threadPool.append(threading.Thread(target=self.B_Handler))
        #一次性启动全部线程
        for thread in threadPool:
            thread.setDaemon(True)  #设置守护线程以防阻塞
            thread.start()
    #接收端函数##################################################################################
    def L_Handler(self):
        '''
        左脚数据同步
        '''
        while self.alive:
            jsonDat,addr=self.l_client.recvfrom(512)
            #   解包为字典型数据
            jsonData = json.loads(jsonDat)
            #   获取网络延时
            self.lFoot.latency.Refreshlatency(jsonData['stamp'])
            #   更新信息
            self.lFoot.UpdateFBData(jsonData)

    def R_Handler(self):
        '''
        右脚数据同步
        '''
        while self.alive:
            #   比特数据
            jsonDat,addr=self.r_client.recvfrom(512)
            #   解包为字典型数据
            jsonData = json.loads(jsonDat)
            #   获取网络延时
            self.rFoot.latency.Refreshlatency(jsonData['stamp'])
            #   更新信息
            self.rFoot.UpdateFBData(jsonData)
    def B_Handler(self):
        '''
        base_link数据同步
        '''
        while self.alive:
            jsonDat,addr=self.b_client.recvfrom(512)
            #   解包为字典型数据
            jsonData = json.loads(jsonDat)
            #   获取网络延时
            self.base.latency.Refreshlatency(jsonData['stamp'])
            #   更新信息
            self.base.UpdateFBData(jsonData)
    ##############发送端函数####################################################
    def SyncJoint(self,l,r,h=0,t=1.0):
        '''
        UDP同步关节数据
        :return:
        '''
        datPush={}
        datPush["type"]="ctrl"
        datPush["l"]=l.copy()
        datPush["r"] = r.copy()
        datPush["l"][2]*=-1
        datPush["l"][3]*=-1
        datPush["l"][1]*=-1
        datPush["h"] = h
        datPush["t"] = t
        datPush=json.dumps(datPush,default=lambda x: round(x, 4))
        self.j_client.sendto(datPush.encode(), (self.ip_base, self.port_jointcontrol))
    def ReleaseJoints(self,l=[1,2,3,4,5,6],r=[1,2,3,4,5,6]):
        '''
        释放机器人关节
        :param l:
        :param r:
        :return:
        '''
        datPush = {}
        datPush["type"] = "free"
        datPush["l"] = l
        datPush["r"] = r

        datPush = json.dumps(datPush)
        self.j_client.sendto(datPush.encode(), (self.ip_base, self.port_jointcontrol))
    def GetJointVals(self,timeout=0.5):
        '''
        获取关节数据
        :return: 成功Flag，左，右
        '''
        datPush = {}
        datPush["type"] = "readall"
        datPush["servo_nums"] = 13      #这个需要修改
        datPush = json.dumps(datPush)
        self.j_client.sendto(datPush.encode(), (self.ip_base, self.port_jointcontrol))
        self.j_client.settimeout(timeout)
        jsonDat, addr = self.j_client.recvfrom(512)
        print(jsonDat)
        dat=json.loads(jsonDat)
        l=np.array(dat['dat'][0:6])
        r=np.array(dat['dat'][6:12])
        h = dat['dat'][12]
        return l,r,h


    def StampSync(self,rate=30):
        '''
        初始化时间戳
        :return:
        '''
        datPush=json.dumps({"type":"config","rate":rate})
        #   分别初始化三个时间戳
        self.rFoot.latency.Reset()
        self.r_client.sendto(datPush.encode(),(self.ip_rfoot,self.port_stampsync))
        self.lFoot.latency.Reset()
        self.l_client.sendto(datPush.encode(), (self.ip_lfoot, self.port_stampsync))
        self.base.latency.Reset()
        self.b_client.sendto(datPush.encode(), (self.ip_base, self.port_stampsync))

    def VoiceOut(self,data,speed=0,volume=8,timbre=0,mood=0):
        '''
        语音输出
        :param data:
        :param speed:
        :param volume:
        :param timbre:
        :param mood:
        :return:
        '''
        datPush={"speed":speed,"volume":volume,"timbre":timbre,"mood":mood,"data":data}
        datPush = json.dumps(datPush)
        self.v_client.sendto(datPush.encode(), (self.ip_base, self.port_voiceout))